% get_lsrbtfm
% Christopher J. Whalen
% University of Illinois
% Date Created - 09/07/05
% Date Modified - 09/07/05

% Purpose - to find the appropriate least squares rigid body transform to
% align two 3D data sets. original' = R*original + T where
% sum(original'-target) minimizes d^2.

% Input - two Nx3 (where N >= 3) matrices with corresponding points. 

% Output - R the rotational matrix and T the translational matrix. R and T
% correspond to the parameters that will be the least squares fit placing
% the original onto the target. Mostly for our case 'original' is the elp points
% and 'target' is the tai fiducial coordinates.


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


function [R,T] = get_lsrbtfm(original,target)

%fprintf('\nObtaining Least Squares Parameters .... \n');

% Check that original and target have same number of points
if size(original,1) ~= size(target,1)
    fprintf('\nError: Both Inputs (original and target) must be contain the same number of points and be Nx3\n');
end

C_target   = calc_centroid(target);
C_original = calc_centroid(original);

H = zeros(3,3);
for i=1:size(original,1)
    A = (original(i,:)-C_original)'*(target(i,:)-C_target);
    H = H + A;
end
[U,S,V] = svd(H);
R = V*U';                       % Least Squares Rotational Matrix
T = C_target' - R*C_original';  % Least Squares Translational Matrix

% Determining if reflection is necessary
B = [1 1 -1;1 1 -1;1 1 -1];     % flipping column 3
if (det(R) < 0)                 % if det = -1
    % fprintf('\nDet(R) = -1 -> Reflecting Data Set.');
    R = (B.*V)*U';
    T = C_target' - R*C_original';
end

% Error Reduction Assessment
% Transform original -> original_lsrbtfm
for i=1:size(original,1)
    original_lsrbtfm(i,:) = (R*original(i,:)' + T)';
end
avg_map_err_init = mean(sqrt(sum(((target-original        ).^2),2))); % Initial Map Error
avg_map_err_fin  = mean(sqrt(sum(((target-original_lsrbtfm).^2),2))); % Final Map Error

%fprintf('\nInitial Average Map Error: %3.1f mm  ', avg_map_err_init);
%fprintf('\nFinal   Average Map Error: %3.1f mm\n', avg_map_err_fin );


